% 函数功能 输入像素坐标系下的激光点（U,V,1）得到相机坐标系下的（x,y,z）
% u = x_c/z_c * fx + u_0
% v = y_c/z_c * fy + v_0

function temp = laser(p, inter, Rc_1, Tc_1)%点，内参矩阵，旋转矩阵，平移矩阵

%像素坐标系->图像坐标系
%归一化 x_c/z_c = (u - u_0) / fx
normlization(1) = (p(1)-inter(1,3))/inter(1,1);
%归一化 y_c/z_c = (v - v_0) / fy
normlization(2) = (p(2)-inter(2,3))/inter(2,2);

%相机坐标系与世界坐标系有 [x , y , z]' = R * [ X_w , Y_w , Z_w]' + T ,  ( matlab中A' 表示 A的转置 )
%取标定板角点为世界坐标系原点，则标定板上左右点的 Z_w = 0 
%进而 inv(R) * ( [x , y , z]' -  T ) = [ X_w , Y_w , Z_w]'
%进而 inv(R)3 * ( [x , y , z]' -  T3 ) = Z_w = 0   (取R逆的第三行对应的等式)
% r'31 * x + r'32 * y + r'33 * z = r'31 * t31 + r'32 * t32 + r'33 * t32
% a * x + b * y + c * z = d  即为标定板平面,  可以通过R&T求得 a,b,c,d ,再根据归一化的x/z，y/z
% 求得 (x,y,z)

t = pinv(Rc_1); %求伪逆
a = t(3,1);
b = t(3,2);
c = t(3,3);
d = Tc_1(1)*a + Tc_1(2)*b + Tc_1(3)*c;

z = d / (a * normlization(1) + b * normlization(2) + c);
x = z * normlization(1);
y = z * normlization(2);

temp = [x; y; z];